/*  Author: Michael Oritz
 *  Email: mtortiz.mail@gmail.com
 *  Date: 4/1/2010
 *
 *  main.c
 *  This is the main state machine control module for the robomagellan
 *  project.
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include "main.h"
#include "pause_state.h"
#include "done_state.h"
#include "track_state.h"
#include "object_avoidance_state.h"
#include "navigation_state.h"
#include "init_state.h"
#include "console.h"
#include "lcd.h"

const char * USAGE = 
    "main_app [options [args]]\n"
    "options:\n"
    "\t-d -- debug output\n"
    "\t-h -- this message\n"
    "\t-s navigation_speed,object_avoidance_speed,"
    "search_speed,track_speed -- sets each speed respectively\n";

// Must line up w/ STATES in main.h
const char * state_strings[NUM_STATES] = {
    "INIT_STATE",
    "NAVIGATION_STATE",
    "OBJECT_AVOIDANCE_STATE",
    "TRACK_STATE",
    "DONE_STATE",
    "ERROR_STATE",
    "PAUSE_STATE"
};

void exit_routine (int sig);

int main (int argc, char **argv) {
    int current_state;
//#ifdef USE_GPS
#if 1
    FILE * waypoint_file;
    int i=0;
    double * p;
#endif
    char c;
    debug = 0;
#ifdef USE_KILL_SWITCH
    kill_switch_asserted = 1;
#endif

    // Enable Control-C detection
    signal(SIGINT, exit_routine);

    // Set default speeds
    navigation_speed = 5;
    object_avoidance_speed = 5;
    search_speed = 5;
    track_speed = 5;

    // Process command line args
    while ((c = getopt(argc, argv, "dhs:")) != 255) {
        switch(c) {
            case 'd':
                debug = 1;
                break;
            case 'h':
                fprintf(stderr, USAGE);
                exit(0);
            case 's':
                if(!optarg)
                    exit(0);
                if(sscanf(optarg, "%d,%d,%d,%d", &navigation_speed, 
                       &object_avoidance_speed, &search_speed, &track_speed) != 4) {
                    fprintf(stderr,"Insufficient or incorrect args\n");
                    fprintf(stderr, USAGE);
                }
                break;
            case '?':
                exit(0);
                break;
        }
    }

//#ifdef USE_GPS
#if 1
        // Open the waypoint file for reading
        if(!(waypoint_file = fopen("waypoints", "r"))) {
            fprintf(stderr,"main: could not open waypoints file\n");
            exit(0);
        }
        // Read in the number of waypoints
        fscanf(waypoint_file, "num_waypoints = %d", &num_waypoints);
        fgetc(waypoint_file);

        // Make space in memory for waypoint values
        if(!(waypoints = (double *)malloc(num_waypoints*2*sizeof(double)))) {
            fprintf(stderr,"main: malloc failed\n");
            exit(0);
        }
        // Read in waypoint coordinates
        for(i=0,p=waypoints; i<num_waypoints; ++i, p+=2) {
            fscanf(waypoint_file, "target_lat =%lf\ntarget_long = %lf", p, p+1);
            fgetc(waypoint_file);
        }
        // Set initial target waypoint
        state_data.target_lat = waypoints[current_waypoint_idx];
        state_data.target_long = waypoints[current_waypoint_idx+1];

#endif

    // Begin with init state
    current_state = next_state = INIT_STATE;

    // Initially no errors
    snprintf(state_data.error_str, sizeof(state_data.error_str), "NO_ERROR");

    while (1) {
        prev_state = current_state;
        current_state = next_state;
        strcpy(state_data.current_state_str, STATE_STR(current_state));
        switch (current_state) {
            case INIT_STATE:
                if(debug) {
                    // Set LCD to white 
                    set_lcd_color("255,255,255");
                    write_lcd("INIT_STATE", 0, 0);
                }
                init_state();
                break;
            case NAVIGATION_STATE:
                if(debug) {
                    // Set LCD to light green
                    set_lcd_color("124,252,0");
                    write_lcd("NAVIGATION_STATE", 0, 0);
                }
                navigation_state();
                break;
            case OBJECT_AVOIDANCE_STATE:
                if(debug) {
                    // Set LCD to light blue
                    set_lcd_color("0,191,255");
                    write_lcd("OBJECT_AVOIDANCE_STATE", 0, 0);
                }
                object_avoidance_state();
                break;
            case TRACK_STATE:
                if (debug) {
                    // Set LCD to orange-red 
                    set_lcd_color("255,69,0");
                    write_lcd("TRACK_STATE", 0, 0);
                }
                track_state();
                break;
            case DONE_STATE:
                if (debug) {
                    // Set LCD to purple
                    set_lcd_color("160,32,240");
                    write_lcd("DONE_STATE", 0, 0);
                }
                done_state();
#ifdef USE_GPS
                free(waypoints);
#endif
                exit(0);
                break;
            case PAUSE_STATE:
                if(debug) {
                    // Set LCD to black 
                    set_lcd_color("0,0,0");
                    write_lcd("PAUSE_STATE", 0, 0);
                }
                pause_state();
                break;
            case ERROR_STATE:
                if (debug) {
                    // Set LCD to firebrick red
                    set_lcd_color("178,34,34");
                    write_lcd("ERROR_STATE", 0, 0);
                    write_lcd(state_data.error_str, 1, 0);
                    write_lcd("Please check device connection and reboot", 2, 0);
                }
                while(1)
                    sleep(1);
                break;
        }
        usleep(1000);
    }
}

void exit_routine (int sig) {
#ifdef USE_GPS
    free(waypoints);
#endif
    // Go to done state to stop car
    if (debug) {
        // Set LCD to gray 
        set_lcd_color("105,105,105");
        write_lcd("You Have Terminated Program Execution", 0, 0);
    }
    strcpy(state_data.current_state_str, "DONE_STATE");
    done_state();
    exit(0);
}
